#include "cpp_co_location_client/Co_Location_Computer.hpp"

double data3[] = {10,0,
         0,10};//pixel noise cov
Eigen::Map<Eigen::Matrix<double, 2, 2>> temp_r(data3);
const Eigen::MatrixXd default_Ri = temp_r;


bool Co_Location_Computer::CreateNewAirplane(Eigen::Vector3d t_g,
            		   Rotation_Angles Pod_Rotation_Angles,
					   uint8_t AirplaneId,
                       Gimbal_Camera Gimbal_camera_parameter,Eigen::MatrixXd Ri)
{
    if(airplanes_.find(AirplaneId)!=airplanes_.end())
    {
        return true;
    }
    // 如果存在不可增删的飞机，则不允许增加飞机
    for(const auto &m:ForbidAddOrDeleteTarget_map_)
    {
        if(m.second)
        {
            return false;
        }
    }
    // 使用 emplace 避免默认构造函数问题
    airplanes_.emplace(AirplaneId, Airplane(t_g, Pod_Rotation_Angles, Gimbal_camera_parameter, Ri));

    // 初始化保护信息
    ForbidAddOrDeleteAirplane_map_[AirplaneId] = false;
    return true;
}

// 删除飞机 此方法的调用需谨慎
// 需增加保护机制
bool Co_Location_Computer::DeleteAirplane(uint8_t AirplaneId)
{
    if(ForbidAddOrDeleteAirplane_map_[AirplaneId])
    {
        return false;
    }
    if(airplanes_.find(AirplaneId)==airplanes_.end())
    {
        return true;
    }
    std::unordered_map<uint8_t,Airplane>::iterator airplane_ptr;
    airplane_ptr = airplanes_.find(AirplaneId);

    airplanes_.erase(airplane_ptr);
    std::unordered_map<uint8_t,bool>::iterator it;
    it = ForbidAddOrDeleteAirplane_map_.find(AirplaneId);
    ForbidAddOrDeleteAirplane_map_.erase(it);
    return true;
}

bool Co_Location_Computer::ChangeAirplanePayload(uint8_t AirplaneId, uint8_t payload_type, uint8_t detail_type, std::vector<double> Datas)
{
    // ForbidAddOrDeleteAirplane_map_[AirplaneId] = true;
    auto it = airplanes_.find(AirplaneId);
    if (it != airplanes_.end()) {
        Airplane &airplane = it->second;
        switch (payload_type)
        {
        case 1:
            /* code */
            {
                switch (detail_type)
                {
                case 1:
                    /* code */
                    {
                    // Gimbal_Camera camera;
                    // camera.f = Datas[0];
                    // camera.d = Datas[1];
                    // airplane.SetGimbal_camera_parameter_(camera);
                    }
                    break;
                case 2:
                    /* code */
                    // airplane.SetGimbal_camera_parameter_d(Datas[0]);
                    break;
                case 3:
                    /* code */
                    // airplane.SetGimbal_camera_parameter_f(Datas[0]);
                    break;
                default:
                    break;
                }
            }
            break;
        case 2:
            /* code */
            {
                switch (detail_type)
                {
                case 1:
                {
                    /* code */
                    Rotation_Angles angles;
                    angles.roll = Datas[0];
                    angles.pitch = Datas[1];
                    angles.yaw = Datas[2];
                    airplane.SetPod_Rotation_Angles_(angles);
                }
                    break;
                case 2:
                    /* code */
                    airplane.SetPod_Rotation_Angles_roll(Datas[0]);
                    break;
                case 3:
                    /* code */
                    airplane.SetPod_Rotation_Angles_pitch(Datas[0]);
                    break;
                case 4:
                    /* code */
                    airplane.SetPod_Rotation_Angles_yaw(Datas[0]);
                    break; 
                default:
                    break;
                }
            }
            break;
        case 3:
            /* code */
            {
                switch (detail_type)
                {
                case 1:
                {
                    /* code */
                    Eigen::Vector3d podtrans;
                    podtrans<<Datas[0],Datas[1],Datas[2];
                    airplane.SetPodTranslation_(podtrans);
                }
                    break;
                case 2:
                    /* code */
                    airplane.SetPodTranslation_x(Datas[0]);
                    break;
                case 3:
                    /* code */
                    airplane.SetPodTranslation_y(Datas[0]);
                    break;
                case 4:
                    /* code */
                    airplane.SetPodTranslation_z(Datas[0]);
                    break;
                default:
                    break;
                }
            }
            break;
        case 4:
            /* code */
            {
                switch (detail_type)
                {
                case 1:
                    /* code */
                {   
                    airplane.SetNoiseCov_u(Datas[0]);
                    airplane.SetNoiseCov_v(Datas[1]);
                }
                    break;
                case 2:
                    /* code */
                    airplane.SetNoiseCov_u(Datas[0]);
                    break;
                case 3:
                    /* code */
                    airplane.SetNoiseCov_v(Datas[0]);
                    break;
                default:
                    break;
                }
            }
            break;
        
        default:
            break;
        }
        // ForbidAddOrDeleteAirplane_map_[AirplaneId] = false;
        return true;
    }
    else
    {
        return false;
    }
}

std::vector<uint8_t> Co_Location_Computer::GetAllAirplanesId()
{
    std::vector<uint8_t> AllAirplanesId;
    // // 取数据时不允许对飞机数量及当前目标的数据集执行增减
    // // 实际两个map的key值是一致的，因此实际也可以直接遍历ForbidAddOrDeleteAirplane_map_来获取ID值
    // for(auto& it : ForbidAddOrDeleteAirplane_map_)
    // {
    //     it.second = true;
    // }
    for(const auto& airplane : airplanes_)
    {
        AllAirplanesId.emplace_back(airplane.first);
    }
    // for(auto& it : ForbidAddOrDeleteAirplane_map_)
    // {
    //     it.second = false;
    // }
    return AllAirplanesId;
}

bool Co_Location_Computer::AirplaneIsCreated(uint8_t AirplaneId)
{
    for(const auto& airplane : airplanes_)
    {
        if(airplane.first == AirplaneId)
            return true;
    }
    return false;
}

std::vector<uint8_t> Co_Location_Computer::GetAllTargetsId()
{
    std::vector<uint8_t> AllTargetsId;
    for(auto& it : ForbidAddOrDeleteTarget_map_)
    {
        // it.second = true;
        AllTargetsId.emplace_back(it.first);
    }
    // for(auto& it : ForbidAddOrDeleteTarget_map_)
    // {
    //     it.second = false;
    // }
    return AllTargetsId;
}

bool Co_Location_Computer::CreateNewTarget(std::vector<uint8_t> &failCreateTarget_Airplane, uint8_t TargetId, uint64_t Time_measure)
{
    if(ForbidAddOrDeleteTarget_map_[TargetId])
    {
        failCreateTarget_Airplane = GetAllAirplanesId();
        return false;
    }
    // 如果该目标已存在，则无需新建
    if (std::find(TargetsId_.begin(), TargetsId_.end(), TargetId) != TargetsId_.end())
    {
        return true;
    }

    // // 遍历时不允许增删
    // for(auto& it : ForbidAddOrDeleteAirplane_map_)
    // {
    //     it.second = true;
    // }
    bool success = true;
    for (auto& airplane : airplanes_)
    {
        if((airplane.second.UpdateTargetLostInfo_Map_(TargetId, Time_measure))==0)
        {
            failCreateTarget_Airplane.emplace_back(airplane.first);
            success = false;
        }
        else
        {
            StepChooseMap_[TargetId] = false;
            // 所有与目标id相关成员需要扩展
            TargetsId_.emplace_back(airplane.first);
            TargetLostInfo_Map_[TargetId];
            // S2没写构造函数
            ukf_initalizer_[TargetId];
            // std::vector<std::string> model = {{"CV"},{"CA"},{"CT"}};
            // Eigen::MatrixXd P = Eigen::MatrixXd::Identity(10,10) * INITIAL_P_SIGMA;
            imm_ukf_filters_[TargetId];
            Estimate_Results_[TargetId];
            ForbidAddOrDeleteTarget_map_[TargetId] = false;
        }
    }
    // for(auto& it : ForbidAddOrDeleteAirplane_map_)
    // {
    //     it.second = false;
    // }
    return success;
}

bool Co_Location_Computer::TargetIsCreated(uint8_t TargetId)
{
    if (std::find(TargetsId_.begin(), TargetsId_.end(), TargetId) != TargetsId_.end())
    {
        return true;
    }
    else
    {
        return false;
    }
}

bool Co_Location_Computer::DeleteTarget(std::vector<uint8_t> &failDeleteTarget_Airplane, uint8_t TargetId)
{
    if(ForbidAddOrDeleteTarget_map_[TargetId])
    {
        failDeleteTarget_Airplane = GetAllAirplanesId();
        return false;
    }
    // 如果该目标不存在，则无需删除
    if (std::find(TargetsId_.begin(), TargetsId_.end(), TargetId) == TargetsId_.end())
    {
        return true;
    }
    bool success = true;
    // // 遍历时不允许增删
    // for(auto& it : ForbidAddOrDeleteAirplane_map_)
    // {
    //     it.second = true;
    // }
    for (auto& airplane : airplanes_)
    {
        bool Success = airplane.second.DeleteTarget(TargetId);
        if(!Success)
        {
            failDeleteTarget_Airplane.emplace_back(airplane.first);
            Success = false;
        }
        else
        {
            StepChooseMap_.erase(TargetId);
            // 所有与目标id相关成员需要删除
            TargetLostInfo_Map_.erase(TargetId);
            ukf_initalizer_.erase(TargetId);
            imm_ukf_filters_.erase(TargetId);
            Estimate_Results_.erase(TargetId);
            ForbidAddOrDeleteTarget_map_.erase(TargetId);
            std::vector<uint8_t>::iterator iter = TargetsId_.begin();
            iter = find(TargetsId_.begin(), TargetsId_.end(), TargetId);
		    if (iter != TargetsId_.end())
		    {
			    TargetsId_.erase(iter);
		    }

        }
    }
    // for(auto& it : ForbidAddOrDeleteAirplane_map_)
    // {
    //     it.second = false;
    // }
    return success;
}



// 数据接收
// 返回值： 0更新失败   1需运行S2   2需运行S3
uint8_t Co_Location_Computer::UpdateAirplaneState(Eigen::Vector3d t_n, Eigen::Vector3d t_c,
                         Rotation_Angles Gimbal_Rotation_Angles, Rotation_Angles Body_Rotation_Angles,
                         uint8_t TargetId, uint8_t AirplaneId,
                         Picture_Location PictureLocation, uint64_t Time_measure)
{
    // 先判断 Computer 存储信息中，是否所有目标丢失, 是则运行 S2
    // S2: ukf is not initialized
    // 再更新飞机丢失信息的数据，并根据飞机当前数据运行 UpdateTargetLostInfo_Map_
    // 若飞机数据更新后发现目标丢失，则运行 S2
    // 未丢失则运行 S3
    // ForbidAddOrDeleteTarget_map_[TargetId] = true;
    // ForbidAddOrDeleteAirplane_map_[AirplaneId] = true;
    bool TargetIsAllLost = TargetLostInfo_Map_[TargetId].TargetIsLost;
    uint8_t Result = 0;

    // 检查 AirplaneId 是否存在
    auto airplane_it = airplanes_.find(AirplaneId);
    if (airplane_it == airplanes_.end())
    {
        // // 找不到对应的 AirplaneId，处理错误或返回
        // ForbidAddOrDeleteTarget_map_[TargetId] = false;
        // ForbidAddOrDeleteAirplane_map_[AirplaneId] = false;
        return Result; // 返回 0 或适当的错误码
    }

    if (!TargetIsCreated(TargetId))
    {
        std::vector<uint8_t> failCreateTarget_Airplane;
        CreateNewTarget(failCreateTarget_Airplane, TargetId, Time_measure);
    }

    // 通过迭代器获取 airplane 引用
    Airplane& airplane = airplane_it->second;

    bool init = (imm_ukf_filters_[TargetId].IsInitialized());
    // std::cout<<"is initialized:"<<int(init)<<std::endl;
    // S2: ukf is not initialized
    if (TargetIsAllLost || (!(init)))
    {
        if (airplane.UpdateAirplaneState(t_n, t_c,
                                         Gimbal_Rotation_Angles, Body_Rotation_Angles,
                                         TargetId, true, 
                                         PictureLocation, Time_measure))
        {
            airplane.UpdateTargetLostInfo_Map_(TargetId, Time_measure);
            // 更新整体丢失信息
            UpdateTargetLostInfo_Map_(TargetId, Time_measure);
            Result = 1;
            StepChooseMap_[TargetId] = false;
        }
    }
    else
    {
        airplane.UpdateTargetLostInfo_Map_(TargetId, Time_measure);
        UpdateTargetLostInfo_Map_(TargetId, Time_measure);
        TargetIsAllLost = TargetLostInfo_Map_[TargetId].TargetIsLost;
        if (airplane.UpdateAirplaneState(t_n, t_c,
                                         Gimbal_Rotation_Angles, Body_Rotation_Angles,
                                         TargetId, TargetIsAllLost,
                                         PictureLocation, Time_measure))
        {
            if (TargetIsAllLost)
            {
                Result = 1;
                StepChooseMap_[TargetId] = false;
            }
            else
            {
                Result = 2;
                StepChooseMap_[TargetId] = true;
            }
        }
    }

    // ForbidAddOrDeleteTarget_map_[TargetId] = false;
    // ForbidAddOrDeleteAirplane_map_[AirplaneId] = false;
    return Result;
}


// 输出数据 当前保存的数据
bool Co_Location_Computer::GetEstimate_Results_(std::vector<Filtered_Target_State> &Estimate_Results, uint8_t TargetId)
{
    // 检查 TargetId 是否存在于 Estimate_Results_ 中
    auto it = Estimate_Results_.find(TargetId);
    if (it != Estimate_Results_.end())
    {
        // 如果找到，则将对应的结果赋给 Estimate_Results
        Estimate_Results = it->second;
        return true;
    }
    return false;
}

// // // 只使用time_now - time_delta 之后的量测数据
// // // 暂时不使用 
// // bool Co_Location_Computer::UpdateTargetState(bool RunUKF, uint64_t time_now, uint64_t time_delta, uint8_t TargetId)
// // {

// // }

// 更新目标的状态
bool Co_Location_Computer::UpdateTargetState(uint8_t TargetId)
{
    bool RunUKF = StepChooseMap_[TargetId];
    bool success = false;
    // 遍历时不允许增删
    ForbidAddOrDeleteTarget_map_[TargetId] = true;
    for(auto& it : ForbidAddOrDeleteAirplane_map_)
    {
        it.second = true;
    }

    if(!RunUKF)
    {
        // 如果运行S2，则取出S2数据所有数据 (有一个数据成功取出就认定取出成功)
        std::vector<DDXTJC_Input> S2_Inputs;
        for(auto& airplane : airplanes_)
        {
            DDXTJC_Input S2_Input;
            if(airplane.second.GetPicture_Location_(S2_Input, TargetId))
            {
                S2_Inputs.push_back(S2_Input);
                success = true;
            }
        }
        if(success)
        {
            // 将数据输入S2，获得结果
            Eigen::Vector3d TargetPosition;
            uint64_t meanTime;
            // 同时将S2的结果输入ukf，执行初始化
            if(ukf_initalizer_[TargetId].update_Position(TargetPosition, meanTime,
                                            S2_Inputs))
            {
                if(!TargetIsCreated(TargetId))
                {
                    if(CreateNewTarget(TargetId, meanTime))
                    {
                        std::cout<<"target is created!"<<std::endl;
                    }
                }
                std::cout<<"S2 success"<<std::endl;
                std::cout<<"S2 Target Position is:"<<TargetPosition<<std::endl;
                if(!(imm_ukf_filters_[TargetId].IMM_Initialization(TargetPosition, meanTime)))
                {
                    success = false;
                    std::cout<<"S3 Initialization fail"<<std::endl;
                }
                else
                {
                    std::cout<<"S3 Initialization success"<<std::endl;
                    Estimate_Results_[TargetId].emplace_back(imm_ukf_filters_[TargetId].Get_Merged_UKF_Output_());
                }
            }
            else
            {
                std::cout<<"S2 computation fail"<<std::endl;
                success = false;
            }
        }
    }
    else
    {
        // 运行S3
        success = UKF_Process(TargetId, DELTA_TIME_FOR_UKF_PROCRSS);
        // 若ukf发散，则不保存UKF数据
        // if(!success)
        // {
        //     StepChooseMap_[TargetId] = false;
        // }
    }
    // 遍历时不允许增删
    for(auto& it : ForbidAddOrDeleteAirplane_map_)
    {
        it.second = false;
    }
    ForbidAddOrDeleteTarget_map_[TargetId] = false;

    return success;
}

// bool Co_Location_Computer::Isinitalized(uint8_t TargetId)
// {
//     return is_initalized_[TargetId];
// }

void Co_Location_Computer::SetTargetLostInfoTargetLostTimeMax(uint8_t TargetId, uint64_t TargetLostTimeMax)
{
    ForbidAddOrDeleteTarget_map_[TargetId] = true;
    TargetLostInfo_Map_[TargetId].TargetLostTimeMax = TargetLostTimeMax;
    ForbidAddOrDeleteTarget_map_[TargetId] = false;
}

void Co_Location_Computer::SetTargetLostInfoTargetLostTimeMaxForOneAirplane(uint8_t AirplaneId, uint8_t TargetId, uint64_t TargetLostTimeMax)
{
    // 保护机制，防止在更新期间增加或删除目标和飞机
    ForbidAddOrDeleteTarget_map_[TargetId] = true;
    ForbidAddOrDeleteAirplane_map_[AirplaneId] = true;

    // 查找指定的 AirplaneId
    auto airplane_it = airplanes_.find(AirplaneId);
    if (airplane_it == airplanes_.end())
    {
        // 找不到对应的 AirplaneId，解除保护并返回
        ForbidAddOrDeleteTarget_map_[TargetId] = false;
        ForbidAddOrDeleteAirplane_map_[AirplaneId] = false;
        // 这里可以返回，记录错误，或者抛出异常
        return;
    }

    // 获取飞机对象
    Airplane& airplane = airplane_it->second;

    // 调用飞机的 SetTargetLostInfoTargetLostTimeMax 方法
    airplane.SetTargetLostInfoTargetLostTimeMax(TargetId, TargetLostTimeMax);

    // 解除保护
    ForbidAddOrDeleteTarget_map_[TargetId] = false;
    ForbidAddOrDeleteAirplane_map_[AirplaneId] = false;
}




// 
/**
 ********** 私有函数 *********
 */


void Co_Location_Computer::UpdateTargetLostInfo_Map_(uint8_t TargetId, uint64_t Time_measure)
{
    uint64_t LastMeasureTime = 0;
    // 从每个飞机中获取综合数据状态，更新数据
    for(auto& airplane:airplanes_)
    {
        TargetLostInfo airplaneTargetLostInfo;
        airplaneTargetLostInfo = airplane.second.GetTargetLostInfo(TargetId);
        // 获得最晚的量测时间,即所有飞机中最后一次观测到此目标
        if(airplaneTargetLostInfo.LastTimeFoundTarget > LastMeasureTime)
        {
            LastMeasureTime = airplaneTargetLostInfo.LastTimeFoundTarget;
        }
    }
    // 比较量测时间， 确定是否丢失
    if(Time_measure - LastMeasureTime > TargetLostInfo_Map_[TargetId].TargetLostTimeMax)
    {
        // 目标已丢失
        TargetLostInfo_Map_[TargetId].TargetIsLost = true;
    }
    else
    {
        // 目标未丢失
        TargetLostInfo_Map_[TargetId].TargetIsLost = false;
    }
    TargetLostInfo_Map_[TargetId].LastTimeFoundTarget = Time_measure;
}

// bool Co_Location_Computer::Initialization(uint8_t TargetId)
// {

// }


// 从每个飞机取数据，
// 将所有数据根据时间间隔进行分块，再整体输入IMM-UKF中
bool Co_Location_Computer::UKF_Process(uint8_t TargetId, uint64_t delta_t)
{
    // // 取数据时不允许对飞机数量及当前目标的数据集执行增减
    // for(auto& it : ForbidAddOrDeleteAirplane_map_)
    // {
    //     it.second = true;
    // }
    // ForbidAddOrDeleteTarget_map_[TargetId] = true;
    // std::unordered_map<uint8_t,Airplane> airplanes = airplanes_;
    if(airplanes_.size()==0)
    {
        return false;
    }
    std::unordered_map<uint8_t,std::vector<Measurement_Pair>> DifferentAirplane_Measurements;
    //每台飞机遍历到的量测数据索引
    std::unordered_map<uint8_t,uint8_t> MeasurementInds; 
    std::vector<Measurement_Pair> MeasurementsThisPeriod;
    std::vector<uint8_t> MeasurementsAirplaneId;
    uint8_t Complete = 0;
    uint8_t AirplaneNum = 0;
    int DataNum = 0;

    //取出所有数据
    for (auto& airplane : airplanes_) { 
        DifferentAirplane_Measurements[airplane.first].clear();
        if(airplane.second.GetMeasurements_(DifferentAirplane_Measurements[airplane.first],TargetId))
        {
            // std::vector<Measurement_Pair> measurements = airplane.second.Measurements_[TargetId];
            std::cout<<"first Measurements size is:"<<int(DifferentAirplane_Measurements[airplane.first].size())<<std::endl;
            // 排序
            std::sort(DifferentAirplane_Measurements[airplane.first].begin(),DifferentAirplane_Measurements[airplane.first].end());
            // 初始化量测数据指针
            MeasurementInds[airplane.first] = 0;
            AirplaneNum++;
            DataNum += int(DifferentAirplane_Measurements[airplane.first].size());
        }
    }
    std::cout<<"AirplaneNum is:"<<int(AirplaneNum)<<std::endl;
    std::cout<<"Measurement size is:"<<DataNum<<std::endl;
    if(!AirplaneNum)
    {
        std::cout<<"No airplane!"<<std::endl;
        return false;
    }
    if(!DataNum)
    {
        std::cout<<"No data!"<<std::endl;
        return false;
    }
    std::vector<Eigen::MatrixXd> RiVector;
    std::vector<MeasureMents_In_Period> UKF_Inputs;//生成输入IMM_UKF的数据集
    Eigen::Vector2d airplane_xy;
    airplane_xy.fill(0.0);

    uint64_t stdTime = TargetLostInfo_Map_[TargetId].LastTimeFoundTarget + TargetLostInfo_Map_[TargetId].TargetLostTimeMax + 1;
    // 遍历每台飞机，顺序判断其测量时间，每个循环生成一个MeasureMents_In_Period并放入UKF_Inputs
    while(Complete < AirplaneNum)
    {
        // std::cout<<"Compute stdTime"<<std::endl;
        stdTime = TargetLostInfo_Map_[TargetId].LastTimeFoundTarget + TargetLostInfo_Map_[TargetId].TargetLostTimeMax + 1;
        // 遍历所有有数据的飞机, 第一轮遍历第一个元素获得基准时间，第二轮取出这台飞机所有在当前时间段内的量测
        // 飞机id，第MeasurementInd.second个量测数据
        for(auto& MeasurementInd : MeasurementInds)
        {
            if(MeasurementInd.second < DifferentAirplane_Measurements[MeasurementInd.first].size())
            {  
                //先遍历计算基准时间  
                Measurement_Pair measurement = DifferentAirplane_Measurements[MeasurementInd.first][MeasurementInd.second];
                //遍历所有飞机的当前组第一个量测，取最小值作为基准时间
                if(measurement.Time_measure < stdTime)
                {
                    stdTime = measurement.Time_measure;
                }
            }
        }
        
        Complete = 0;
        // std::cout<<"Next Turn!"<<std::endl;
        for(auto& MeasurementInd : MeasurementInds)
        {
            //数据索引未指到头
            // std::cout<<"MeasurementInd second is:"<<int(MeasurementInd.second)<<std::endl;
            // std::cout<<"Measurements size is:"<<int(DifferentAirplane_Measurements[MeasurementInd.first].size())<<std::endl;
            if(MeasurementInd.second < DifferentAirplane_Measurements[MeasurementInd.first].size())
            {   
                // std::cout<<"AirplaneID is:"<<int(MeasurementInd.first)<<std::endl;
                
                Measurement_Pair measurement = DifferentAirplane_Measurements[MeasurementInd.first][MeasurementInd.second];
                //针对MeasurementInd.first编号的飞机，取出所有在这个期间内的数据
                while((measurement.Time_measure <= stdTime + delta_t)&&(MeasurementInd.second < DifferentAirplane_Measurements[MeasurementInd.first].size()))
                {
                    if(!measurement.MeasurementMatrix.isZero())
                    {
                        // std::cout<<"MeasurementInd second is:"<<int(MeasurementInd.second)<<std::endl;
                        MeasurementsThisPeriod.push_back(measurement);
                        auto it = airplanes_.find(MeasurementInd.first);
                        if (it != airplanes_.end()) {
                            // 如果键存在，访问对象并调用 GetRi_() 函数
                            RiVector.push_back(it->second.GetRi_());
                        }

                        airplane_xy += measurement.airplane_xy;
                    }
                    MeasurementInd.second++;
                } 
            }
            else{
                Complete++;
            }
        }
        if(MeasurementsThisPeriod.size()>0)
        {
            // std::cout<<"Complete Num is:"<<int(Complete)<<std::endl;
            MeasureMents_In_Period A_UKF_Input = Generate_UKF_Input(MeasurementsThisPeriod, RiVector);
            MeasurementsThisPeriod.clear();
            UKF_Inputs.push_back(A_UKF_Input);
        }
    }
    airplane_xy/=DataNum;
    // std::cout<<"airplane_xy is:"<<airplane_xy<<std::endl;
    // for(auto& it : ForbidAddOrDeleteAirplane_map_)
    // {
    //     it.second = true;
    // }
    // ForbidAddOrDeleteTarget_map_[TargetId] = true;

    // 数据输入UKF
    if(AirplaneNum)
    {
        Filtered_Target_State IMM_Output;
        if(imm_ukf_filters_[TargetId].Process(IMM_Output,UKF_Inputs))
        {
            std::cout<<"UKF process success!"<<std::endl;
            auto diff = IMM_Output.State.head(2) - airplane_xy;
            std::cout<<"diff.norm()"<<diff.norm()<<std::endl;
            if(diff.norm()<500.0)
            {
                Estimate_Results_[TargetId].push_back(IMM_Output);
                while(Estimate_Results_[TargetId].size() > MaxSavedEstimationNum){
                    Estimate_Results_[TargetId].erase(Estimate_Results_[TargetId].begin());
                }
                return true;
            }
            else
            {
                imm_ukf_filters_[TargetId].TargetLost();
                return false;
            }
        }
        else{
            
            return false;
        }
    }
    else{
        return false;
    }
}



// 此处的引用不做返回值
/**
 * 用于生成IMM_UKF的一个时间片段的输入结果
 */
MeasureMents_In_Period Co_Location_Computer::Generate_UKF_Input(std::vector<Measurement_Pair> &Measurements, std::vector<Eigen::MatrixXd> &Ri)
{
    MeasureMents_In_Period UKF_Inputs;
    uint8_t PeriodNum = Ri.size();
    if(PeriodNum == Measurements.size())
    {
        Eigen::VectorXd v(PeriodNum*2);
        UKF_Inputs.MeasurementsVector = v;
        /**
         * @brief 根据该时间段内的量测数据数量，和对应飞机生成合并的矩阵R
         * 		根据测量时间分组结果，将飞机的Ri融合
         */
        UKF_Inputs.R_merge = Generte_R_merge_(Ri, PeriodNum);
        UKF_Inputs.meanTime = 0;
        int Ind = 0;
        for (auto &measurement : Measurements)
        {
            /**
             * @brief 生成meanTime
             */
            UKF_Inputs.meanTime += measurement.Time_measure;
            /**
             * @brief 生成Eigen::VectorXd MeasurementsVector
             */
            UKF_Inputs.MeasurementsVector(Ind) = measurement.PictureLocation.u;
            Ind++;
            UKF_Inputs.MeasurementsVector(Ind) = measurement.PictureLocation.v;
            Ind++;
            /**
             * @brief 生成MeasurementsMatrix
             */
            UKF_Inputs.MeasurementsMatrix.push_back(measurement.MeasurementMatrix);
            UKF_Inputs.Airplane_Z.push_back(measurement.airplane_z);
        }
        UKF_Inputs.meanTime /= PeriodNum;
    }
    return UKF_Inputs;
}

// 此处的引用不做返回值
Eigen::MatrixXd Co_Location_Computer::Generte_R_merge_(std::vector<Eigen::MatrixXd> &Ri, uint8_t PeriodNum)
{
    //按Ri的顺序，将Ri排列在合并R矩阵的对角线上
    Eigen::MatrixXd R_merge(PeriodNum*2,PeriodNum*2);
    for (uint8_t i = 0;i<PeriodNum;i++)
    {
        R_merge.block<2,2>((i*2), (i*2)) = Ri[i];
    }
    return R_merge;
}

bool Co_Location_Computer::CreateNewTarget(uint8_t TargetId, uint64_t Time_measure)
{
    //     // 遍历时不允许增删
    // for(auto& it : ForbidAddOrDeleteAirplane_map_)
    // {
    //     it.second = true;
    // }
    bool success = true;
    for (auto& airplane : airplanes_)
    {
        if((airplane.second.UpdateTargetLostInfo_Map_(TargetId, Time_measure))==false)
        {
            success = false;
        }
        else
        {
            // 所有与目标id相关成员需要扩展
            TargetsId_.emplace_back(airplane.first);
            TargetLostInfo_Map_[TargetId];
            // S2没写构造函数
            ukf_initalizer_[TargetId];
            std::vector<std::string> model = {{"CV"},{"CA"},{"CT"}};
            Eigen::MatrixXd P = Eigen::MatrixXd::Identity(10,10) * INITIAL_P_SIGMA;
            imm_ukf_filters_[TargetId];
            Estimate_Results_[TargetId];
            ForbidAddOrDeleteTarget_map_[TargetId] = false;
        }
    }
    // for(auto& it : ForbidAddOrDeleteAirplane_map_)
    // {
    //     it.second = false;
    // }
    return success;
}

// 定义全局共享对象
// std::shared_ptr<Co_Location_Computer> AI_Co_Location_Computer = std::make_shared<Co_Location_Computer>();